#include <Arduino.h>
#include "HardwareTimer.h"
#include "analog.h"

extern __IO uint32_t NVIC_Priority_Group;

#define TIM_SET_COMPARE(__HANDLE__, __CHANNEL__, __COMPARE__)                                                                                                  \
    (((__CHANNEL__) == TIM_Channel_1) ? TIM_SetCompare1(__HANDLE__, __COMPARE__) : ((__CHANNEL__) == TIM_Channel_2) ? TIM_SetCompare2(__HANDLE__, __COMPARE__) \
                                                                               : ((__CHANNEL__) == TIM_Channel_3)   ? TIM_SetCompare3(__HANDLE__, __COMPARE__) \
                                                                                                                    : (TIM_SetCompare4(__HANDLE__, __COMPARE__)))

#define TIM_GET_COMPARE(__HANDLE__, __CHANNEL__)                                                                                     \
    (((__CHANNEL__) == TIM_Channel_1) ? TIM_GetCapture1(__HANDLE__) : ((__CHANNEL__) == TIM_Channel_2) ? TIM_GetCapture2(__HANDLE__) \
                                                                  : ((__CHANNEL__) == TIM_Channel_3)   ? TIM_GetCapture3(__HANDLE__) \
                                                                                                       : (TIM_GetCapture4(__HANDLE__)))

static inline void TIM_OCInit(TIM_TypeDef *TIMx, TIM_OCInitTypeDef *TIM_OCInitStruct, uint16_t channel)
{
    switch (channel)
    {
    case TIM_Channel_1:
        TIM_OC1Init(TIMx, TIM_OCInitStruct);
        TIM_OC1PreloadConfig(TIMx, TIM_OCPreload_Disable);
        break;
    case TIM_Channel_2:
        TIM_OC2Init(TIMx, TIM_OCInitStruct);
        TIM_OC2PreloadConfig(TIMx, TIM_OCPreload_Disable);
        break;
    case TIM_Channel_3:
        TIM_OC3Init(TIMx, TIM_OCInitStruct);
        TIM_OC3PreloadConfig(TIMx, TIM_OCPreload_Disable);
        break;
    case TIM_Channel_4:
        TIM_OC4Init(TIMx, TIM_OCInitStruct);
        TIM_OC4PreloadConfig(TIMx, TIM_OCPreload_Disable);
        break;
    default:
        break;
    }
}

#define PIN_NOT_USED 0xFF
#define MAX_RELOAD ((1 << 16) - 1)

timerObj_t *HardwareTimer_Handle[TIMER_NUM] = {NULL};

static inline timerObj_t *get_timer_obj(TIM_TypeDef *instance)
{
    timerObj_t *obj = NULL;
    for (int i = 0; i < TIMER_NUM; i++)
    {
        if (HardwareTimer_Handle[i] && HardwareTimer_Handle[i]->instance == instance)
        {
            obj = HardwareTimer_Handle[i];
            break;
        }
    }
    return (obj);
}

HardwareTimer::HardwareTimer(TIM_TypeDef *instance)
{
    timer_index_t index = get_timer_index(instance);

    if (index == UNKNOWN_TIMER)
        Error_Handler();

    HardwareTimer_Handle[index] = &_timerObj;

    _timerObj.__this = (void *)this;
    _timerObj.idx = index;
    _timerObj.instance = instance;
    _timerObj.preemptPriority = TIM_IRQ_PRIO;
    _timerObj.subPriority = TIM_IRQ_SUBPRIO;

    enableTimerClock(index);

    for (int i = 0; i < TIMER_CHANNELS; i++)
    {
        callbacks[i] = NULL;
    }

    for (int i = 0; i < TIMER_CHANNELS; i++)
    {
        _ChannelMode[i] = TIMER_DISABLED;
    }

    TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;

    TIM_TimeBaseInitStructure.TIM_Period = MAX_RELOAD;              //设置自动重装载寄存器的值
    TIM_TimeBaseInitStructure.TIM_Prescaler = 0;                    //设置定时器预分频器值
    TIM_TimeBaseInitStructure.TIM_ClockDivision = TIM_CKD_DIV1;     //设置时钟分频因子，配置死区时间时需要用到
    TIM_TimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up; //设置计数器计数模式，向上计数
    TIM_TimeBaseInitStructure.TIM_RepetitionCounter = 0x00;         //设置重复计数器的值，没用到不用管

    TIM_TimeBaseInit(_timerObj.instance, &TIM_TimeBaseInitStructure); //初始化定时器
}

HardwareTimer::~HardwareTimer()
{
    timer_index_t index = get_timer_index(_timerObj.instance);
    disableTimerClock(index);

    HardwareTimer_Handle[index] = NULL;
    _timerObj.__this = NULL;
}

void HardwareTimer::pause(void)
{
    TIM_ITConfig(_timerObj.instance, TIM_IT_Update, DISABLE);
    TIM_ITConfig(_timerObj.instance, TIM_IT_CC1, DISABLE);
    TIM_ITConfig(_timerObj.instance, TIM_IT_CC2, DISABLE);
    TIM_ITConfig(_timerObj.instance, TIM_IT_CC3, DISABLE);
    TIM_ITConfig(_timerObj.instance, TIM_IT_CC4, DISABLE);

    uint32_t TIMIRQn = getTimerIrq(_timerObj.instance);
    if (NonMaskableInt_IRQn != TIMIRQn)
    {
        NVIC_InitTypeDef NVIC_InitStructure;
        //初始化TIM NVIC，设置中断优先级分组
        NVIC_InitStructure.NVIC_IRQChannel = TIMIRQn;    //TIM3中断
        NVIC_InitStructure.NVIC_IRQChannelCmd = DISABLE; //使能通道1中断
        NVIC_Init(&NVIC_InitStructure);                  //初始化NVIC
    }

    TIM_Cmd(_timerObj.instance, DISABLE);
}

void HardwareTimer::pauseChannel(uint32_t channel)
{
    int timChannel = getChannel(channel);
    if (timChannel == -1)
        Error_Handler();

    int interrupt = getIT(channel);
    if (interrupt == -1)
        Error_Handler();

    TIM_ITConfig(_timerObj.instance, interrupt, DISABLE);
    TIM_CCxCmd(_timerObj.instance, timChannel, TIM_CCx_Disable);
}

void HardwareTimer::resume(void)
{
    if (callbacks[0])
    {
        TIM_ClearFlag(_timerObj.instance, TIM_FLAG_Update);
        TIM_ITConfig(_timerObj.instance, TIM_IT_Update, ENABLE);
    }

    resumeChannel(1);
    resumeChannel(2);
    resumeChannel(3);
    resumeChannel(4);

    uint32_t TIMIRQn = getTimerIrq(_timerObj.instance);
    if (NonMaskableInt_IRQn != TIMIRQn)
    {
        NVIC_InitTypeDef NVIC_InitStructure;
        //初始化TIM NVIC，设置中断优先级分组
        NVIC_InitStructure.NVIC_IRQChannel = TIMIRQn;                        //TIM3中断
        NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = TIM_IRQ_PRIO; //设置抢占优先级0
        NVIC_InitStructure.NVIC_IRQChannelSubPriority = TIM_IRQ_SUBPRIO;     //设置响应优先级3
        NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;                      //使能通道1中断
        NVIC_Init(&NVIC_InitStructure);                                      //初始化NVIC
    }

    TIM_Cmd(_timerObj.instance, ENABLE);
}

void HardwareTimer::resumeChannel(uint32_t channel)
{
    int timChannel = getChannel(channel);
    if (timChannel == -1)
        Error_Handler();

    int interrupt = getIT(channel);
    if (interrupt == -1)
        Error_Handler();

    int itFlag = getITFlag(channel);
    if (itFlag == -1)
        Error_Handler();

    if (callbacks[channel])
    {
        TIM_ClearFlag(_timerObj.instance, itFlag);
        TIM_ITConfig(_timerObj.instance, interrupt, ENABLE);
    }
}

void HardwareTimer::setPrescaleFactor(uint32_t prescaler)
{
    (_timerObj.instance)->PSC = prescaler;
}

uint32_t HardwareTimer::getPrescaleFactor()
{
    return TIM_GetPrescaler(_timerObj.instance);
}

void HardwareTimer::setOverflow(uint32_t overflow, TimerFormat_t format)
{
    uint32_t ARR_RegisterValue;
    uint32_t PeriodTicks;
    uint32_t period_cyc;
    uint32_t Prescalerfactor;

    switch (format)
    {
    case MICROSEC_FORMAT:
        period_cyc = overflow * (SystemCoreClock / 1000000);
        Prescalerfactor = (period_cyc / 0x10000) + 1;
        TIM_PrescalerConfig(_timerObj.instance, Prescalerfactor - 1, TIM_PSCReloadMode_Update);
        PeriodTicks = period_cyc / Prescalerfactor;
        break;
    case HERTZ_FORMAT:
        period_cyc = SystemCoreClock / overflow;
        Prescalerfactor = (period_cyc / 0x10000) + 1;
        TIM_PrescalerConfig(_timerObj.instance, Prescalerfactor - 1, TIM_PSCReloadMode_Update);
        PeriodTicks = period_cyc / Prescalerfactor;
        break;
    case TICK_FORMAT:
    default:
        PeriodTicks = overflow;
        _timerObj.instance->PSC = PeriodTicks;
        break;
    }

    if (PeriodTicks > 0)
        ARR_RegisterValue = PeriodTicks - 1;
    else
        ARR_RegisterValue = 0;

    TIM_SetAutoreload(_timerObj.instance, ARR_RegisterValue);
}

uint32_t HardwareTimer::getOverflow(TimerFormat_t format)
{
    uint32_t ARR_RegisterValue = _timerObj.instance->ATRLR;
    uint32_t Prescalerfactor = TIM_GetPrescaler(_timerObj.instance) + 1;
    uint32_t return_value;

    switch (format)
    {
    case MICROSEC_FORMAT:
        return_value = (uint32_t)(((ARR_RegisterValue + 1) * Prescalerfactor * 1000000.0) / SystemCoreClock);
        break;
    case HERTZ_FORMAT:
        return_value = (uint32_t)(SystemCoreClock / ((ARR_RegisterValue + 1) * Prescalerfactor));
        break;
    case TICK_FORMAT:
    default:
        return_value = ARR_RegisterValue + 1;
        break;
    }
    return return_value;
}

void HardwareTimer::setPWM(uint32_t channel, PinName pin, uint32_t frequency, uint32_t dutycycle, callback_function_t PeriodCallback, callback_function_t CompareCallback)
{
    setMode(channel, TIMER_OUTPUT_COMPARE_PWM1, pin);
    setOverflow(frequency, HERTZ_FORMAT);
    setCaptureCompare(channel, dutycycle, PERCENT_COMPARE_FORMAT);

    if (PeriodCallback)
    {
        attachInterrupt(PeriodCallback);
    }
    if (CompareCallback)
    {
        attachInterrupt(channel, PeriodCallback);
    }

    resume();
}

void HardwareTimer::setPWM(uint32_t channel, uint32_t pin, uint32_t frequency, uint32_t dutycycle, callback_function_t PeriodCallback, callback_function_t CompareCallback)
{
    setPWM(channel, digitalPinToPinName(pin), frequency, dutycycle, PeriodCallback, CompareCallback);
}

void HardwareTimer::setCount(uint32_t counter, TimerFormat_t format)
{
    uint32_t CNT_RegisterValue;
    uint32_t Prescalerfactor = TIM_GetPrescaler(_timerObj.instance) + 1;

    switch (format)
    {
    case MICROSEC_FORMAT:
        CNT_RegisterValue = ((counter * (SystemCoreClock / 1000000)) / Prescalerfactor);
        break;
    case HERTZ_FORMAT:
        CNT_RegisterValue = (uint32_t)(SystemCoreClock / (counter * Prescalerfactor));
        break;
    case TICK_FORMAT:
    default:
        CNT_RegisterValue = counter;
        break;
    }

    TIM_SetCounter(_timerObj.instance, CNT_RegisterValue);
}

uint32_t HardwareTimer::getCount(TimerFormat_t format)
{
    uint32_t CNT_RegisterValue = TIM_GetCounter(_timerObj.instance);
    uint32_t Prescalerfactor = TIM_GetPrescaler(_timerObj.instance) + 1;
    uint32_t return_value;
    switch (format)
    {
    case MICROSEC_FORMAT:
        return_value = (uint32_t)((CNT_RegisterValue * Prescalerfactor * 1000000.0) / SystemCoreClock);
        break;
    case HERTZ_FORMAT:
        return_value = (uint32_t)(SystemCoreClock / (CNT_RegisterValue * Prescalerfactor));
        break;
    case TICK_FORMAT:
    default:
        return_value = CNT_RegisterValue;
        break;
    }
    return return_value;
}

void HardwareTimer::setMode(uint32_t channel, TimerModes_t mode, PinName pin)
{
    int timChannel = getChannel(channel);
    if (timChannel == -1)
        Error_Handler();

    TIM_OCInitTypeDef channelOC;
    TIM_ICInitTypeDef channelIC;
    _ChannelMode[channel - 1] = mode;

    channelOC.TIM_OCMode = TIMER_NOT_USED;
    channelOC.TIM_Pulse = 0x0000;
    channelOC.TIM_OCPolarity = TIM_OCPolarity_High;
    channelOC.TIM_OCNPolarity = TIM_OCPolarity_High;
    channelOC.TIM_OCIdleState = TIM_OCIdleState_Reset;
    channelOC.TIM_OCNIdleState = TIM_OCNIdleState_Reset;

    channelIC.TIM_Channel = timChannel;
    channelIC.TIM_ICPolarity = TIMER_NOT_USED;
    channelIC.TIM_ICSelection = TIM_ICSelection_DirectTI;
    channelIC.TIM_ICPrescaler = TIM_ICPSC_DIV1;
    channelIC.TIM_ICFilter = 0x00;

    switch (mode)
    {
    case TIMER_DISABLED:
        channelOC.TIM_OCMode = TIM_OCMode_Timing;
        TIM_OCInit(_timerObj.instance, &channelOC, timChannel);
        break;
    case TIMER_OUTPUT_COMPARE:
        channelOC.TIM_OCMode = TIM_OCMode_Timing;
        TIM_OCInit(_timerObj.instance, &channelOC, timChannel);
        break;
    case TIMER_OUTPUT_COMPARE_ACTIVE:
        channelOC.TIM_OCMode = TIM_OCMode_Active;
        TIM_OCInit(_timerObj.instance, &channelOC, timChannel);
        break;
    case TIMER_OUTPUT_COMPARE_INACTIVE:
        channelOC.TIM_OCMode = TIM_OCMode_Inactive;
        TIM_OCInit(_timerObj.instance, &channelOC, timChannel);
        break;
    case TIMER_OUTPUT_COMPARE_TOGGLE:
        channelOC.TIM_OCMode = TIM_OCMode_Toggle;
        TIM_OCInit(_timerObj.instance, &channelOC, timChannel);
        break;
    case TIMER_OUTPUT_COMPARE_PWM1:
        channelOC.TIM_OCMode = TIM_OCMode_PWM1;
        channelOC.TIM_OutputState = TIM_OutputState_Enable;
        TIM_OCInit(_timerObj.instance, &channelOC, timChannel);
        TIM_CtrlPWMOutputs(_timerObj.instance, ENABLE);
        break;
    case TIMER_OUTPUT_COMPARE_PWM2:
        channelOC.TIM_OCMode = TIM_OCMode_PWM2;
        channelOC.TIM_OutputState = TIM_OutputState_Enable;
        TIM_OCInit(_timerObj.instance, &channelOC, timChannel);
        TIM_CtrlPWMOutputs(_timerObj.instance, ENABLE);
        break;
        // case TIMER_OUTPUT_COMPARE_FORCED_ACTIVE:
        //     channelOC.TIM_OCMode = TIM_OCMode_PWM2;
        //     TIM_OCInit(_timerObj.instance, &channelOC, timChannel);
        //     break;
        // case TIMER_OUTPUT_COMPARE_FORCED_INACTIVE:
        //     channelOC.TIM_OCMode = TIM_OCMode_PWM2;
        //     TIM_OCInit(_timerObj.instance, &channelOC, timChannel);
        //     break;
    case TIMER_INPUT_CAPTURE_RISING:
        channelIC.TIM_ICPolarity = TIM_ICPolarity_Rising;
        TIM_ICInit(_timerObj.instance, &channelIC);
        break;
    case TIMER_INPUT_CAPTURE_FALLING:
        channelIC.TIM_ICPolarity = TIM_ICPolarity_Falling;
        TIM_ICInit(_timerObj.instance, &channelIC);
        break;
    case TIMER_INPUT_CAPTURE_BOTHEDGE:
        channelIC.TIM_ICPolarity = TIM_ICPolarity_BothEdge;
        TIM_ICInit(_timerObj.instance, &channelIC);
        break;
        // case TIMER_INPUT_FREQ_DUTY_MEASUREMENT:
        //     channelIC.TIM_ICPolarity = TIM_ICPolarity_Rising;
        //     channelIC.TIM_ICSelection = TIM_ICSelection_DirectTI;
        //     TIM_ICInit(_timerObj.instance, &channelIC);
        //     break;
    }

    if (pin != NC)
    {
        if (get_pwm_channel(pin) == timChannel)
        {
            if (
                mode == TIMER_INPUT_CAPTURE_RISING || mode == TIMER_INPUT_CAPTURE_FALLING ||
                mode == TIMER_INPUT_CAPTURE_BOTHEDGE /*|| mode == TIMER_INPUT_FREQ_DUTY_MEASUREMENT*/)
            {
                pinMode(pin, INPUT);
            }
            else
            {
                pinmap_pinout(pin, PinMap_PWM);
            }
            TIM_ARRPreloadConfig(_timerObj.instance, ENABLE);
        }
    }
}

void HardwareTimer::setMode(uint32_t channel, TimerModes_t mode, uint32_t pin)
{
    setMode(channel, mode, digitalPinToPinName(pin));
}

TimerModes_t HardwareTimer::getMode(uint32_t channel)
{
    if ((1 <= channel) && (channel <= TIMER_CHANNELS))
    {
        return _ChannelMode[channel - 1];
    }
    else
    {
        return TIMER_DISABLED;
    }
}

void HardwareTimer::setPreloadEnable(bool value)
{
    if (value)
    {
        TIM_ARRPreloadConfig(_timerObj.instance, ENABLE);
    }
    else
    {
        TIM_ARRPreloadConfig(_timerObj.instance, DISABLE);
    }
}

uint32_t HardwareTimer::getCaptureCompare(uint32_t channel, TimerCompareFormat_t format)
{
    int timChannel = getChannel(channel);
    if (timChannel == -1)
        Error_Handler();

    uint32_t CCR_RegisterValue = TIM_GET_COMPARE(_timerObj.instance, timChannel);
    uint32_t Prescalerfactor = TIM_GetPrescaler(_timerObj.instance) + 1;
    uint32_t return_value;

    switch (format)
    {
    case MICROSEC_COMPARE_FORMAT:
        return_value = (uint32_t)((CCR_RegisterValue * Prescalerfactor * 1000000.0) / SystemCoreClock);
        break;
    case HERTZ_COMPARE_FORMAT:
        return_value = (uint32_t)(SystemCoreClock / (CCR_RegisterValue * Prescalerfactor));
        break;
    case PERCENT_COMPARE_FORMAT:
        return_value = (CCR_RegisterValue * 100) / _timerObj.instance->ATRLR;
        break;
    case RESOLUTION_1B_COMPARE_FORMAT:
    case RESOLUTION_2B_COMPARE_FORMAT:
    case RESOLUTION_3B_COMPARE_FORMAT:
    case RESOLUTION_4B_COMPARE_FORMAT:
    case RESOLUTION_5B_COMPARE_FORMAT:
    case RESOLUTION_6B_COMPARE_FORMAT:
    case RESOLUTION_7B_COMPARE_FORMAT:
    case RESOLUTION_8B_COMPARE_FORMAT:
    case RESOLUTION_9B_COMPARE_FORMAT:
    case RESOLUTION_10B_COMPARE_FORMAT:
    case RESOLUTION_11B_COMPARE_FORMAT:
    case RESOLUTION_12B_COMPARE_FORMAT:
    case RESOLUTION_13B_COMPARE_FORMAT:
    case RESOLUTION_14B_COMPARE_FORMAT:
    case RESOLUTION_15B_COMPARE_FORMAT:
    case RESOLUTION_16B_COMPARE_FORMAT:
        return_value = (CCR_RegisterValue * ((1 << format) - 1)) / _timerObj.instance->ATRLR;
        break;
    case TICK_COMPARE_FORMAT:
    default:
        return_value = CCR_RegisterValue;
        break;
    }
    return return_value;
}

void HardwareTimer::setCaptureCompare(uint32_t channel, uint32_t compare, TimerCompareFormat_t format)
{
    int timChannel = getChannel(channel);
    if (timChannel == -1)
        Error_Handler();

    uint32_t Prescalerfactor = TIM_GetPrescaler(_timerObj.instance) + 1;
    uint32_t ARR_RegisterValue = _timerObj.instance->ATRLR;
    uint32_t CCR_RegisterValue;

    switch (format)
    {
    case MICROSEC_COMPARE_FORMAT:
        CCR_RegisterValue = ((compare * (SystemCoreClock / 1000000)) / Prescalerfactor);
        break;
    case HERTZ_COMPARE_FORMAT:
        CCR_RegisterValue = SystemCoreClock / (compare * Prescalerfactor);
        break;
    // As per Reference Manual PWM reach 100% with CCRx value strictly greater than ARR (So ARR+1 in our case)
    case PERCENT_COMPARE_FORMAT:
        CCR_RegisterValue = ((ARR_RegisterValue + 1) * compare) / 100;
        break;
    case RESOLUTION_1B_COMPARE_FORMAT:
    case RESOLUTION_2B_COMPARE_FORMAT:
    case RESOLUTION_3B_COMPARE_FORMAT:
    case RESOLUTION_4B_COMPARE_FORMAT:
    case RESOLUTION_5B_COMPARE_FORMAT:
    case RESOLUTION_6B_COMPARE_FORMAT:
    case RESOLUTION_7B_COMPARE_FORMAT:
    case RESOLUTION_8B_COMPARE_FORMAT:
    case RESOLUTION_9B_COMPARE_FORMAT:
    case RESOLUTION_10B_COMPARE_FORMAT:
    case RESOLUTION_11B_COMPARE_FORMAT:
    case RESOLUTION_12B_COMPARE_FORMAT:
    case RESOLUTION_13B_COMPARE_FORMAT:
    case RESOLUTION_14B_COMPARE_FORMAT:
    case RESOLUTION_15B_COMPARE_FORMAT:
    case RESOLUTION_16B_COMPARE_FORMAT:
        CCR_RegisterValue = ((ARR_RegisterValue + 1) * compare) / ((1 << format) - 1);
        break;
    case TICK_COMPARE_FORMAT:
    default:
        CCR_RegisterValue = compare;
        break;
    }

    if (ARR_RegisterValue == MAX_RELOAD && CCR_RegisterValue == MAX_RELOAD - 1)
    {
        CCR_RegisterValue = MAX_RELOAD;
    }

    TIM_SET_COMPARE(_timerObj.instance, timChannel, CCR_RegisterValue);
}

void HardwareTimer::setInterruptPriority(uint32_t preemptPriority, uint32_t subPriority)
{
    IRQn_Type TIMIRQn = getTimerIrq(_timerObj.instance);
    NVICSetPriority(TIMIRQn, preemptPriority, subPriority);

    if (getTimerCCIrq(_timerObj.instance) != getTimerIrq(_timerObj.instance))
    {
        NVICSetPriority(getTimerCCIrq(_timerObj.instance), preemptPriority, subPriority);
    }
    _timerObj.preemptPriority = preemptPriority;
    _timerObj.subPriority = subPriority;
}

void HardwareTimer::attachInterrupt(callback_function_t callback)
{
    if (callbacks[0])
    {
        callbacks[0] = callback;
    }
    else
    {
        callbacks[0] = callback;
        if (callback)
        {
            TIM_ClearFlag(_timerObj.instance, TIM_FLAG_Update);
            TIM_ITConfig(_timerObj.instance, TIM_IT_Update, ENABLE);
        }
    }
}

void HardwareTimer::detachInterrupt()
{
    TIM_ITConfig(_timerObj.instance, TIM_IT_Update, DISABLE);
    callbacks[0] = NULL;
}

bool HardwareTimer::hasInterrupt()
{
    return callbacks[0] != NULL;
}

void HardwareTimer::attachInterrupt(uint32_t channel, callback_function_t callback)
{
    int interrupt = getIT(channel);
    if (interrupt == -1)
        Error_Handler();

    if ((channel == 0) || (channel > (TIMER_CHANNELS + 1)))
        Error_Handler();

    if (callbacks[channel])
    {
        callbacks[channel] = callback;
    }
    else
    {
        callbacks[channel] = callback;
        if (callback)
        {
            TIM_ClearFlag(_timerObj.instance, interrupt);
            TIM_ITConfig(_timerObj.instance, interrupt, ENABLE);
        }
    }
}

void HardwareTimer::detachInterrupt(uint32_t channel)
{
    int interrupt = getIT(channel);
    if (interrupt == -1)
        Error_Handler();

    if ((channel == 0) || (channel > (TIMER_CHANNELS + 1)))
        Error_Handler();

    TIM_ITConfig(_timerObj.instance, interrupt, DISABLE);
    callbacks[channel] = NULL;
}

bool HardwareTimer::hasInterrupt(uint32_t channel)
{
    if ((channel == 0) || (channel > (TIMER_CHANNELS + 1)))
    {
        Error_Handler();
    }
    return callbacks[channel] != NULL;
}

void HardwareTimer::timerHandleDeinit()
{
    TIM_ITConfig(_timerObj.instance, TIM_IT_Update, DISABLE);
    TIM_DeInit(_timerObj.instance);
}

void HardwareTimer::refresh(void)
{
    TIM_GenerateEvent(_timerObj.instance, TIM_EventSource_Update);
}

uint32_t HardwareTimer::getTimerClkFreq()
{
    uint32_t uwTimclock = 0U;

    RCC_ClocksTypeDef RCC_ClockStruct;
    RCC_GetClocksFreq(&RCC_ClockStruct);

    uwTimclock = RCC_ClockStruct.HCLK_Frequency;

    return uwTimclock;
}

void HardwareTimer::captureCompareCallback(TIM_TypeDef *htim, uint16_t channel)
{
    if (!htim)
        Error_Handler();

    int32_t channel_idx;
    switch (channel)
    {
    case TIM_Channel_1:
        channel_idx = 1;
        break;
    case TIM_Channel_2:
        channel_idx = 2;
        break;
    case TIM_Channel_3:
        channel_idx = 3;
        break;
    case TIM_Channel_4:
        channel_idx = 4;
        break;
    default:
        channel_idx = -1;
        break;
    }

    if (channel_idx < 0)
        return;

    timerObj_t *obj = get_timer_obj(htim);
    HardwareTimer *HT = (HardwareTimer *)(obj->__this);
    if (HT == NULL)
        return;

    if (HT->callbacks[channel_idx])
    {
        HT->callbacks[channel_idx]();
    }
}

void HardwareTimer::updateCallback(TIM_TypeDef *htim)
{
    if (!htim)
        Error_Handler();

    timerObj_t *obj = get_timer_obj(htim);
    HardwareTimer *HT = (HardwareTimer *)(obj->__this);
    if (HT == NULL)
        return;

    if (HT->callbacks[0])
    {
        HT->callbacks[0]();
    }
}

int HardwareTimer::getChannel(uint32_t channel)
{
    uint32_t return_value;
    switch (channel)
    {
    case 1:
        return_value = TIM_Channel_1;
        break;
    case 2:
        return_value = TIM_Channel_2;
        break;
    case 3:
        return_value = TIM_Channel_3;
        break;
    case 4:
        return_value = TIM_Channel_4;
        break;
    default:
        return_value = -1;
    }
    return return_value;
}

int HardwareTimer::getIT(uint32_t channel)
{
    uint32_t return_value;

    switch (channel)
    {
    case 1:
        return_value = TIM_IT_CC1;
        break;
    case 2:
        return_value = TIM_IT_CC2;
        break;
    case 3:
        return_value = TIM_IT_CC3;
        break;
    case 4:
        return_value = TIM_IT_CC4;
        break;
    default:
        return_value = -1;
    }
    return return_value;
}

int HardwareTimer::getITFlag(uint32_t channel)
{
    uint32_t return_value;

    switch (channel)
    {
    case 1:
        return_value = TIM_FLAG_CC1;
        break;
    case 2:
        return_value = TIM_FLAG_CC2;
        break;
    case 3:
        return_value = TIM_FLAG_CC3;
        break;
    case 4:
        return_value = TIM_FLAG_CC4;
        break;
    default:
        return_value = -1;
    }
    return return_value;
}

void HardwareTimer::NVICSetPriority(IRQn_Type IRQn, uint32_t preemptPriority, uint32_t subPriority)
{
    uint8_t tmppre = 0;
    if (NVIC_Priority_Group == NVIC_PriorityGroup_0)
    {
        NVIC_SetPriority(IRQn, subPriority << 4);
    }
    else if (NVIC_Priority_Group == NVIC_PriorityGroup_1)
    {
        if (preemptPriority == 1)
        {
            NVIC_SetPriority(IRQn, (1 << 7) | (subPriority << 4));
        }
        else
        {
            NVIC_SetPriority(IRQn, (0 << 7) | (subPriority << 4));
        }
    }
    else if (NVIC_Priority_Group == NVIC_PriorityGroup_2)
    {
        if (preemptPriority <= 1)
        {
            tmppre = subPriority + (4 * preemptPriority);
            NVIC_SetPriority(IRQn, (0 << 7) | (tmppre << 4));
        }
        else
        {
            tmppre = subPriority + (4 * (preemptPriority - 2));
            NVIC_SetPriority(IRQn, (1 << 7) | (tmppre << 4));
        }
    }
    else if (NVIC_Priority_Group == NVIC_PriorityGroup_3)
    {
        if (preemptPriority <= 3)
        {
            tmppre = subPriority + (2 * preemptPriority);
            NVIC_SetPriority(IRQn, (0 << 7) | (tmppre << 4));
        }
        else
        {
            tmppre = subPriority + (2 * (preemptPriority - 4));
            NVIC_SetPriority(IRQn, (1 << 7) | (tmppre << 4));
        }
    }
    else if (NVIC_Priority_Group == NVIC_PriorityGroup_4)
    {
        NVIC_SetPriority(IRQn, preemptPriority << 4);
    }
}

/******************************************************************************/
/*                            TIMx IRQ HANDLER                                */
/******************************************************************************/
extern "C"
{
    void tim_interrupt_IO_callback(TIM_TypeDef *instance, uint16_t channel)
    {
        HardwareTimer::captureCompareCallback(instance, channel);
    }

    void tim_interrupt_PE_callback(TIM_TypeDef *instance)
    {
        HardwareTimer::updateCallback(instance);
    }

    void tim_interrupt_callback(TIM_TypeDef *instance)
    {
        if (TIM_GetITStatus(instance, TIM_FLAG_CC1) != RESET)
        {
            TIM_ClearITPendingBit(instance, TIM_FLAG_CC1);
            tim_interrupt_IO_callback(instance, TIM_Channel_1);
        }

        if (TIM_GetITStatus(instance, TIM_FLAG_CC2) != RESET)
        {
            TIM_ClearITPendingBit(instance, TIM_FLAG_CC2);
            tim_interrupt_IO_callback(instance, TIM_Channel_2);
        }

        if (TIM_GetITStatus(instance, TIM_FLAG_CC3) != RESET)
        {
            TIM_ClearITPendingBit(instance, TIM_FLAG_CC3);
            tim_interrupt_IO_callback(instance, TIM_Channel_3);
        }

        if (TIM_GetITStatus(instance, TIM_FLAG_CC4) != RESET)
        {
            TIM_ClearITPendingBit(instance, TIM_FLAG_CC4);
            tim_interrupt_IO_callback(instance, TIM_Channel_4);
        }

        if (TIM_GetITStatus(instance, TIM_IT_Update) != RESET)
        {
            TIM_ClearITPendingBit(instance, TIM_IT_Update);
            tim_interrupt_PE_callback(instance);
        }
    }

#if defined(TIM1_BASE)
    void __func_interrupt TIM1_UP_IRQHandler(void)
    {
        if (HardwareTimer_Handle[TIMER1_INDEX])
        {
            tim_interrupt_callback(HardwareTimer_Handle[TIMER1_INDEX]->instance);
        }
    }

    void __func_interrupt TIM1_CC_IRQHandler(void)
    {
        if (HardwareTimer_Handle[TIMER1_INDEX])
        {
            tim_interrupt_callback(HardwareTimer_Handle[TIMER1_INDEX]->instance);
        }
    }
#endif

#if defined(TIM2_BASE)
    void __func_interrupt TIM2_IRQHandler(void)
    {
        if (HardwareTimer_Handle[TIMER2_INDEX])
        {
            tim_interrupt_callback(HardwareTimer_Handle[TIMER2_INDEX]->instance);
        }
    }
#endif

#if defined(TIM3_BASE)
    void __func_interrupt TIM3_IRQHandler(void)
    {
        if (HardwareTimer_Handle[TIMER3_INDEX])
        {
            tim_interrupt_callback(HardwareTimer_Handle[TIMER3_INDEX]->instance);
        }
    }
#endif

#if defined(TIM4_BASE)
    void __func_interrupt TIM4_IRQHandler(void)
    {
        if (HardwareTimer_Handle[TIMER4_INDEX])
        {
            tim_interrupt_callback(HardwareTimer_Handle[TIMER4_INDEX]->instance);
        }
    }
#endif
}